#include "SubsystemPathfinding.h"
#include "Subsystems/SubsystemTerrain.h"
#include "GameSingleton.h"
#include "Managers/BlocksManager.h"

namespace PocketSurvival
{

    void SubsystemPathfinding::pathSearch(const Vector3 &start, const Vector3 &end, float minDistance, const Vector3 &boxSize, int32_t maxPositionsToCheck, PathfindingResult *result)
    {
        result->IsCompleted = false;
        result->IsInProgress = true;

        Request &request = m_astar->World.request;
        request.Start = start;
        request.End = end;
        request.MinDistance = minDistance;
        request.BoxSize = boxSize;
        request.MaxPositionsToCheck = maxPositionsToCheck;
        request.PathFinding = result;

        m_astar->Path = &(request.PathFinding->Path);
        m_astar->findPath(request.Start, request.End, request.MinDistance, request.MaxPositionsToCheck);
        smoothPath(request.PathFinding->Path, request.BoxSize);

        request.PathFinding->PathCost = m_astar->PathCost;
        request.PathFinding->PositionChecked = m_astar->ClosedStorage.storageMap.size();
        request.PathFinding->IsInProgress = false;
        request.PathFinding->IsCompleted = true;
    }

    // 路径平滑
    void SubsystemPathfinding::smoothPath(std::vector<Vector3> &path, const Vector3 &boxSize)
    {
        if(path.size() > 2)
        {
            std::vector<Vector3> result;

            Vector3 front = path[0];
            Vector3 middle = path[1];

            result.push_back(front);

            for(int32_t index = 2; index < path.size(); index++)
            {
                if(isPassable(front, path[index], boxSize))
                {
                    middle = path[index];
                }
                else
                {
                    front = middle;
                    middle = path[index];
                    result.push_back(front);
                }
            }

            path.swap(result);
        }
    }

    bool SubsystemPathfinding::isPassable(const Vector3 &p1, const Vector3 &p2, const Vector3 &boxSize)
    {
        Vector3 vector(p1.posX, p1.posY + 0.5f, p1.posZ);
        Vector3 vector2(p2.posX, p2.posY + 0.5f, p2.posZ);
        Vector3 v = (0.5f * boxSize.posX + 0.1f) * Vector3::Normalize(Vector3::Cross(FastVector3::UnitY, vector2 - vector));
        if (m_terrain->raycast(vector, vector2, false, true, SmoothingRaycastFunction_Obstacle) != nullptr)
        {
            return false;
        }
        if (m_terrain->raycast(vector - v, vector2 - v, false, true, SmoothingRaycastFunction_Obstacle) != nullptr)
        {
            return false;
        }
        if (m_terrain->raycast(vector + v, vector2 + v, false, true, SmoothingRaycastFunction_Obstacle) != nullptr)
        {
            return false;
        }
        if (m_terrain->raycast(vector + Vector3(0.0f, -1.0f, 0.0f), vector2 + Vector3(0.0f, -1.0f, 0.0f), false, false, SmoothingRaycastFunction_Support) != nullptr)
        {
            return false;
        }
        if (m_terrain->raycast(vector + Vector3(0.0f, -1.0f, 0.0f) - v, vector2 + Vector3(0.0f, -1.0f, 0.0f) - v, false, false, SmoothingRaycastFunction_Support) != nullptr)
        {
            return false;
        }
        if (m_terrain->raycast(vector + Vector3(0.0f, -1.0f, 0.0f) + v, vector2 + Vector3(0.0f, -1.0f, 0.0f) + v, false, false, SmoothingRaycastFunction_Support) != nullptr)
        {
            return false;
        }
        return true;
    }

    bool SubsystemPathfinding::SmoothingRaycastFunction_Obstacle(int32_t value, float distance)
    {
        int32_t num = Terrain::ExtractContents(value);
        Block *block = GameSingleton::blocksManager->blocks[num];
        if (block->shouldAvoid(value))
        {
            return true;
        }
        if (block->IsCollidable)
        {
            return true;
        }
        return false;
    }

    bool SubsystemPathfinding::SmoothingRaycastFunction_Support(int32_t value, float distance)
    {
        int32_t num = Terrain::ExtractContents(value);
        Block *block = GameSingleton::blocksManager->blocks[num];
        if (block->shouldAvoid(value))
        {
            return true;
        }
        if (false == block->IsCollidable)
        {
            return true;
        }
        return false;
    }

    static const std::string subsystemName = "Pathfinding";
    const std::string &SubsystemPathfinding::getName() const
    {
        return subsystemName;
    }
    void SubsystemPathfinding::load(const nlohmann::json &json)
    {
        m_terrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();
        m_astar = std::make_shared<AStarVector3>();
    }

} // namespace PocketSurvival
